#include "WorkpieceE618.h"
#include <QThread>
#include <QMap>
#include <unistd.h>

#include "ms618_algbase.h"
#include "ms618_algbackdatum.h"
#include "ms618_algbackl1tol8circle.h"
#include "ms618_algbackrcedge.h"
#include "ms618_algbackd1tod8circle.h"
#include "ms618_algbackse1tose3edge.h"
#include "ms618_algbackss1toss6circle.h"
#include "ms618_algfrontse1tose5edge.h"
#include "ms618_alghighdatum.h"
#include "ms618_algside1datum.h"
#include "ms618_algsidese7edge.h"

using namespace MeasurE618;

WorkpieceE618::WorkpieceE618():
    m_threadStopFlag(false),
    m_isSetPathFlag(false),
    m_initThreadFinished_(false)
{
    m_workpieceBuff.SetSize(MAX_WORKPIECE_BUFFER_NUM);
    if(!m_initThreadFinished_) {
        InitThread();
    }
}

WorkpieceE618::~WorkpieceE618()
{
    ExitThread();
}

void WorkpieceE618::CacheData(const cv::Mat &image         ,
                              const QPoint &datumOffset    ,
                              int workpieceId              ,
                              int pictureId                ,
                              int channelId                ,
                              int fixtureId)
{
    if(!m_workpieceMap.contains(workpieceId))
    {
        WorkpieceInfo workpieceInfo;
        workpieceInfo.workpieceId  = workpieceId            ;
        workpieceInfo.channelId    = channelId              ;
        workpieceInfo.fixtureId    = fixtureId              ;
        m_workpieceMap.insert(workpieceId, workpieceInfo)   ;
    }
    switch (pictureId) {
    case PICTURE_ID_3:
    case PICTURE_ID_4:
    case PICTURE_ID_7:
    case PICTURE_ID_1:
    case PICTURE_ID_8:
        m_workpieceMap[workpieceId].imageMap[pictureId]            = image;
        m_workpieceMap[workpieceId].imageDatumOffsetMap[pictureId] = datumOffset;
        m_workpieceMap[workpieceId].imageNum++;
        m_workpieceMap[workpieceId].curPictureId = pictureId  ;
        break;
    default:
        break;
    }
    // if finished input to queue
    //判断当前图像是否可以计算，如果可以计算，发送信号进行计算
    if(m_workpieceMap[workpieceId].IsPictureUseable(WORKPIECE_TYPE_E618, pictureId)) {
        m_workpieceBuff.SetData(m_workpieceMap[workpieceId]);

        if(m_workpieceMap[workpieceId].IsWorkpieceUseable(WORKPIECE_TYPE_E618)) {
            m_workpieceMap.remove(workpieceId);
        }
        //
        // 此处默认工件号由小到大进行排序，如果当前工件号和最小工件号相差MAX_WAIT_WORKPIECE_NUM个，
        // 说明最小工件号的工件图像已经丢失，
        // 需要删除该工件信息
        QMap<int, WorkpieceInfo>::iterator iter = m_workpieceMap.begin();
        int minWorkpieceId = iter.key();
        if(workpieceId - minWorkpieceId > MAX_WAIT_WORKPIECE_NUM) {
            m_workpieceMap.remove(minWorkpieceId);
            qDebug() << "workpiece : " << workpieceId << "lost";
        }
    }
}

bool WorkpieceE618::SetTemplatePath(const std::string &path)
{
    AlgBase::SetTemplatePath(path);
    m_isSetPathFlag = true;
    return true;
}

bool WorkpieceE618::SetStandardVal(const std::map<std::string, MeasureItemStandardVal> &standardValMap)
{
    std::map<std::string, float> standardValStrMap;
    for(const auto &item: standardValMap) {
        std::string keyStr = item.first;
        standardValStrMap.insert(make_pair(keyStr, item.second.standardVal));
    }
    AlgBase::SetStandardVal(standardValStrMap);
    return true;
}

void WorkpieceE618::SetWorkpieceInfo(const string &modeName, const string &workpieceType)
{
    AlgBase::SetWorkpieceInfo(modeName, workpieceType);
}

void WorkpieceE618::ProcessData()
{
    qDebug() << "start to E618 measure workpiece";
    while(!m_isSetPathFlag) {
        sleep(0.1);
    }

    qDebug() << "E618 measure workpiece";

    AlgBackDatum                    op0(MAX_THREAD_NUM_E618);
    AlgBackL1toL8Circle             op1(MAX_THREAD_NUM_E618);
    AlgBackRCEdge                   op2(MAX_THREAD_NUM_E618);
    AlgBackD1toD8Circle             op3(MAX_THREAD_NUM_E618);
    AlgBackSE1toSE3Edge             op4(MAX_THREAD_NUM_E618);
    AlgBackSS1toSS6Circle           op5(MAX_THREAD_NUM_E618);
    AlgFrontSE1toSE5Edge            op6(MAX_THREAD_NUM_E618);
    AlgHighDatum                    op7(MAX_THREAD_NUM_E618);
    AlgSide1Datum                   op8(MAX_THREAD_NUM_E618); //注意 对应 图片为side2  8号图
    AlgSideSE7Edge                  op9(MAX_THREAD_NUM_E618); //注意 对应 图片为side1  1号图

    int id = -1;

    while(!m_threadStopFlag)
    {
        WorkpieceInfo workpieceInfo;
        if(!m_workpieceBuff.GetData(workpieceInfo)) {
            qDebug() << "current thread exit" << QThread::currentThreadId();
            return;
        }

        // process data
        cv::Mat imgBack  = workpieceInfo.imageMap[PICTURE_ID_3].clone();
        cv::Mat imgFront = workpieceInfo.imageMap[PICTURE_ID_4].clone();
        cv::Mat imgHigh  = workpieceInfo.imageMap[PICTURE_ID_7].clone();
        cv::Mat imgSide1 = workpieceInfo.imageMap[PICTURE_ID_1].clone(); //注意 对应 AlgSideSE7Edge
        cv::Mat imgSide2 = workpieceInfo.imageMap[PICTURE_ID_8].clone(); //注意 对应 AlgSide1Datum

        int workpieceId = workpieceInfo.workpieceId;
        int channelId   = workpieceInfo.channelId  ;
        int fixtureId   = workpieceInfo.fixtureId  ;

#ifdef MEASURE_TIME
        auto RecordStartTime = [&](int workpieceId, int pictureId) {
            QString key = QString("%1-%2").arg(workpieceId).arg(pictureId);
            QTime *startTime = new QTime();
            g_measurementCostTime[key] = startTime;
            startTime->start();
        };

        RecordStartTime(workpieceId, PICTURE_ID_3);
        RecordStartTime(workpieceId, PICTURE_ID_4);
        RecordStartTime(workpieceId, PICTURE_ID_7);
        RecordStartTime(workpieceId, PICTURE_ID_8);
        RecordStartTime(workpieceId, PICTURE_ID_1);
#endif

        id++;
        id = id % MAX_THREAD_NUM_E618;

        std::map<std::string, float> measRes[7];

        std::thread t0(op0,   std::ref(imgBack) , std::ref(id));
        std::thread t1(op1,   std::ref(imgBack) , std::ref(id), std::ref(measRes[0]), fixtureId);
        std::thread t2(op2,   std::ref(imgBack) , std::ref(id), std::ref(measRes[1]), fixtureId);
        std::thread t3(op3,   std::ref(imgBack) , std::ref(id), std::ref(measRes[2]), fixtureId);
        std::thread t4(op4,   std::ref(imgBack) , std::ref(id), std::ref(measRes[3]), fixtureId);
        std::thread t5(op5,   std::ref(imgBack) , std::ref(id), std::ref(measRes[4]), fixtureId);
        std::thread t6(op6,   std::ref(imgFront), std::ref(id), std::ref(measRes[5]), fixtureId);
        std::thread t7(op7,   std::ref(imgHigh) , std::ref(id));
        std::thread t8(op8,   std::ref(imgSide2), std::ref(id));
        std::thread t9(op9,   std::ref(imgSide1), std::ref(id), std::ref(measRes[6]), fixtureId);

        t0.join();
        t1.join();
        t2.join();
        t3.join();
        t4.join();
        t5.join();
        t6.join();
        t7.join();
        t8.join();
        t9.join();

//        AlgBase::clearAllflag();

        auto MergeResult = [&](std::map<string, float> &measureInfo,
                std::map<string, float> measureInfoSub) {
            for(const auto &item : measureInfoSub) {
                measureInfo.insert(make_pair(item.first, item.second));
            }
        };

        QMap<QString, QString> error;
        std::map<std::string, float> measResBack;
        MergeResult(measResBack, measRes[0]) ;
        MergeResult(measResBack, measRes[1]) ;
        MergeResult(measResBack, measRes[2]) ;
        MergeResult(measResBack, measRes[3]) ;
        MergeResult(measResBack, measRes[4]) ;

        emit MesurementResultResponse(workpieceId, PICTURE_ID_3, channelId,
                                      fixtureId, measResBack, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_3
                 << "channelId: "
                 << channelId;

        std::map<std::string, float> measResFront;
        MergeResult(measResFront, measRes[5]);

        emit MesurementResultResponse(workpieceId, PICTURE_ID_4, channelId,
                                      fixtureId, measResFront, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_4
                 << "channelId: "
                 << channelId;

        std::map<std::string, float>   measureInfoHigh;
        //MergeResult(measureInfoHigh, measRes[5]);
        emit MesurementResultResponse(workpieceId, PICTURE_ID_7, channelId,
                                      fixtureId,   measureInfoHigh, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_7
                 << "channelId: "
                 << channelId;

        std::map<std::string, float>   measureInfoSide1;
//        MergeResult(measureInfoSide1, measRes9);
        emit MesurementResultResponse(workpieceId, PICTURE_ID_8, channelId,
                                      fixtureId, measureInfoSide1, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_8
                 << "channelId: "
                 << channelId;

        std::map<std::string, float>   measureInfoSide2;
        MergeResult(measureInfoSide2, measRes[6]);
        emit MesurementResultResponse(workpieceId, PICTURE_ID_1, channelId,
                                      fixtureId, measureInfoSide2, error);

        qDebug() << "workpieceId: "
                 << workpieceId
                 << "pictureId:"
                 << PICTURE_ID_1
                 << "channelId: "
                 << channelId;

        AlgBase::clearAllflag(id);
    }
    qDebug() << "end to E618 measure workpiece and quit thread";
}

bool WorkpieceE618::GetDatumPixelDistance(const QImage &img, int fixtureId, QMap<QString, float> &pixelDistance)
{
    return false;
}

void WorkpieceE618::InitThread()
{
    for(int i = 0; i < MAX_THREAD_NUM_MEASURE; i++) {
        qDebug() << "start thread";
        m_threadPool.emplace_back(std::thread(&WorkpieceE618::ProcessData, this));
    }
    qDebug() << "start thread success";
    m_initThreadFinished_ = true;
}

void WorkpieceE618::ExitThread()
{
    if(m_initThreadFinished_) {
        if(!m_threadStopFlag) {
            m_threadStopFlag = true;
        }
        qDebug() << "start to exit thread";

        int loopCount = 0;
        while(!m_workpieceBuff.Empty() && loopCount < 3) {
            sleep(1);
            loopCount++;
        }

        // 调用该语句释放条件变量的等待条件
        m_workpieceBuff.Exit();

        for(size_t i = 0; i < m_threadPool.size(); ++i) {
            m_threadPool[i].join();
        }

        qDebug() << "measurement thread exit safely!";
    }
}

float WorkpieceE618::CalPosition(std::map<string, float> &measRes, string strX, string strY)
{
    if(measRes.empty()) {
        qDebug() << "gray algorithm: E618 caculate position error";
        return -1;
    }
    int endIndexX = strX.find("_real");
    int endIndexY = strY.find("_real");
    std::string standardXStr = strX.substr(0, endIndexX);
    std::string standardYStr = strY.substr(0, endIndexY);
    return 2*sqrt(((measRes[strX] - MeasurE618::AlgBase::m_standardValMap[standardXStr]) * (measRes[strX] - MeasurE618::AlgBase::m_standardValMap[standardXStr]))
                  + ((measRes[strY] - MeasurE618::AlgBase::m_standardValMap[standardYStr]) * (measRes[strY] - MeasurE618::AlgBase::m_standardValMap[standardYStr])));
}
